#include <climits>
#include <cstdlib>
#include <cmath>
#include <iostream>
#include <list>
#include <map>

#include "Map.h"
#include "Point.h"
#include "Node.h"

using namespace std;

#define FACTOR 10

void  AddAllNodesToSet();
void FindPath();
list<Node *> GetNeighbors(Node* node);
void AddNeighborNode(int diffX, int diffY, Node* parent, list<Node*> *n);
Node* NodeWithLowestScore();
int GetDifference(Node* n, Node* p);

list<Node *> lnodes;

Map astarMap;
map<char, float> gValues;

int main(int argc, char* argv[])
{
  if (argc != 2) {
    cout << "Usage: " << argv[0] << " mapFile" << endl;
    exit(1);
  }

  gValues['-'] = 1.0;
  gValues['M'] = 1.3;
  gValues['S'] = 1.0;
  gValues['E'] = 1.0;

  if (!astarMap.LoadMap(argv[1])) {
    cout << "Unable to load map: " << argv[1] << endl;
    exit(2);
  }
  //astarMap.PrettyPrint();

  AddAllNodesToSet();
  FindPath();

#ifdef GRAPH_VIS
  astarMap.PrintDigraph();
#else
  astarMap.PrintDijkstras();
#endif

  return 0;
}

void FindPath()
{
  int tentative;
  list<Node *> neighbors;
  list<Node *>::iterator nit;

  Node* curNode = astarMap.GetBoardNode(astarMap.GetStartPoint());
  curNode->SetF(0);

  while (!lnodes.empty()) {
    curNode = NodeWithLowestScore();

    if (curNode->GetF() == INT_MAX) {
      break; // All remaining nodes are inaccessible from source
    }

    lnodes.remove(curNode);

    neighbors = GetNeighbors(curNode);

    for (nit = neighbors.begin();
	 nit != neighbors.end();
	 ++nit) {
      tentative = (**nit).GetTentativeParent()->GetF() + 
	GetDifference(*nit, (**nit).GetTentativeParent());

      if (tentative < (**nit).GetF()) {
	(**nit).SetParent(curNode);
	(**nit).SetF(tentative);
      }
    }

  }
}

list<Node*> GetNeighbors(Node* node)
{
  list<Node*> n;

  // Note: (0, 0) is in the top left corner
  AddNeighborNode( 0, -1, node, &n); // North
  AddNeighborNode( 1, -1, node, &n); // Northeast
  AddNeighborNode( 1,  0, node, &n); // East
  AddNeighborNode( 1,  1, node, &n); // Southeast
  AddNeighborNode( 0,  1, node, &n); // South
  AddNeighborNode(-1,  1, node, &n); // Southwest
  AddNeighborNode(-1,  0, node, &n); // West
  AddNeighborNode(-1, -1, node, &n); // Northwest

  return n;
}

void AddNeighborNode(int diffX, int diffY, Node* parent, list<Node*> *n)
{
  Point p  = parent->GetCoords();
  Node* boardNode;

  //cout << "Coords: (" << p.GetX() << ", " << p.GetY() << ") - "
  //     << " x: " << diffX << " | y: " << diffY << endl;

  if (astarMap.IsWalkable(p.GetX() + diffX, p.GetY() + diffY)) {
    if ((boardNode = astarMap.GetBoardNode(p.GetX() + diffX,
					   p.GetY() + diffY)) != NULL) {
      if (boardNode->IsInSet()) {
	//cout << "Adding " << *boardNode
	//   << " | Parent: " << *parent << endl;
	boardNode->SetTentativeParent(parent);
	n->push_back(boardNode);
      }
    }
  }
}

int GetDifference(Node* n, Node* p)
{
  Point p1 = n->GetCoords();
  Point p2 = p->GetCoords();

  double dist = (FACTOR * sqrt(pow((double)(p1.GetX() - p2.GetX()), (double)2) + 
			      pow((double)(p1.GetY() - p2.GetY()), (double)2)));  

  return (int)(gValues.find(n->GetData())->second * dist);
}

void AddAllNodesToSet()
{
  for (unsigned int y = 0; y < astarMap.GetBoardY(); ++y) {
    for (unsigned int x = 0; x < astarMap.GetBoardX(); ++x) {
      lnodes.push_back(astarMap.GetBoardNode(x, y));
    }
  }
}

Node* NodeWithLowestScore()
{
  Node* n = lnodes.front();
  for (list<Node*>::iterator lit = lnodes.begin();
       lit != lnodes.end();
       ++lit) {
    if ((**lit).GetF() < n->GetF()) {
      n = *lit;
    }
  }

  return n;
}
